#include "ros/ros.h"
#include "ros/package.h"
#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include "sensor_msgs/Image.h"
#include "kinfu_slam/KeyFrameMsg.h"
#include "kinfu_slam/SequencedPoseMsg.h"
#include <cv_bridge/cv_bridge.h>
#include "GL/glew.h"
#include "GL/freeglut.h"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include "LoadShaders.h"
#include "DOpenGLCamera.h"
#include "DOpenGLPointCloud.h"
#include "DOpenGLPoseViewer.h"

using namespace cv;
using namespace std;
using namespace Eigen;

void keyframeDataCallback(const kinfu_slam::KeyFrameMsg::ConstPtr& msg);
void keyframePoseOptimizedCallback(const kinfu_slam::SequencedPoseMsg::ConstPtr& msg);
void trackerPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void uavPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
bool InitGL();
void initPointcloud();


DOpenGLPointCloud backgroundPointCloud;
DOpenGLCamera dCamera;
DOpenGLPoseViewer poseView;

GLuint ShadingProgram;
GLuint render_projectionMatrix_position;
GLuint render_viewMatrix_position;
GLuint render_modelMatrix_position;

const float StaticDisplayCubeRes = 0.8f;
const int StaticDisplayCubeLength_X = 100;
const int StaticDisplayCubeLength_Y = 40;
const int StaticDisplayCubeLength_Z = 80;
//const int StaticDisplayCubeVertexListLength = 4*(StaticDisplayCubeLength_X*StaticDisplayCubeLength_Y + StaticDisplayCubeLength_X*StaticDisplayCubeLength_Z + StaticDisplayCubeLength_Y*StaticDisplayCubeLength_Z);
const int StaticDisplayCubeVertexListLength = 4*(StaticDisplayCubeLength_X*StaticDisplayCubeLength_Z);
float staticDisplayCubeVertexList[StaticDisplayCubeVertexListLength*3];
float staticDisplayCubeNormList[StaticDisplayCubeVertexListLength*3];
float staticDisplayCubeColorList[StaticDisplayCubeVertexListLength*3];
bool global_exit = false;

const int MaxKeyFrameNum = 200;
DOpenGLPointCloud kfPointCloud[MaxKeyFrameNum];
bool kfShouldDraw[MaxKeyFrameNum];
bool pointcloudLoaded[MaxKeyFrameNum];

float tmp_nans[640*480*3];

int main(int argc, char **argv)
{
	ros::init(argc, argv, "viewer");
	ros::NodeHandle nh;


	if(!InitGL())
	{
		ROS_ERROR("Inint OpenGL failed");
		return 1;
	};
	initPointcloud();
	poseView.Init();
	poseView.InitCamModel(0.25,0.15,0.35);

	for(int i=0;i<640*480*3;i++)
	{
		tmp_nans[i] = std::numeric_limits<float>::quiet_NaN();
	}


	ros::Subscriber kf_full_data_sub = nh.subscribe("kinfu_slam/kf_full_data", 10, keyframeDataCallback);
	ros::Subscriber kf_pose_sub = nh.subscribe("kinfu_slam/kf_pose_optimized", 100, keyframePoseOptimizedCallback);
	ros::Subscriber tracker_pose_sub = nh.subscribe("kinfu_slam/tracker_pose", 10, trackerPoseCallback);
	//ros::Subscriber uav_pose_sub = nh.subscribe("mavros/local_position/pose", 10, uavPoseCallback);
	while(ros::ok())
	{
		ros::spinOnce();

		glutMainLoopEvent();
	}

	return 0;
}


void keyframeDataCallback(const kinfu_slam::KeyFrameMsg::ConstPtr& msg)
{
	int rows = msg->rows;
	int cols = msg->cols;
	int cur_keyframe_id = msg->keyframeSeq;

	ROS_INFO("KeyFrame data recieved: id = [%d] rows = [%d] cols = [%d]",cur_keyframe_id, rows, cols);

	Affine3d kf_pose;
	tf::poseMsgToEigen(msg->pose,kf_pose);
	Matrix3f pose_R = kf_pose.rotation().cast<float>();
	Vector3f pose_t = kf_pose.translation().cast<float>();
	kfPointCloud[cur_keyframe_id].SetModelMatrix(pose_R.data(), pose_t.data());

	int kf_vertex_num = rows*cols;

	const float *kf_vertex_list = &msg->data[0];
	const float *kf_norm_list = &msg->data[0]+3*kf_vertex_num;
	const float *kf_color_list = &msg->data[0]+6*kf_vertex_num;
	//kfPointCloud[cur_keyframe_id].SetModelMatrix(kf_head->pose_R,kf_head->pose_T);

	kfPointCloud[cur_keyframe_id].LoadPointCloud(kf_vertex_list,3,kf_vertex_num,GL_STATIC_DRAW);
	kfPointCloud[cur_keyframe_id].LoadPointCloudNorm(kf_norm_list,3,kf_vertex_num,GL_STATIC_DRAW);
	kfPointCloud[cur_keyframe_id].LoadPointCloudColor(kf_color_list,3,kf_vertex_num,GL_STATIC_DRAW);
	kfShouldDraw[cur_keyframe_id] = true;

	glutPostRedisplay();

	ROS_INFO("KeyFrame data set");
}

void keyframePoseOptimizedCallback(const kinfu_slam::SequencedPoseMsg::ConstPtr& msg)
{
	int cur_keyframe_id = msg->keyframeSeq;
	ROS_INFO("KeyFrame optimized pose recieved: id = [%d]",cur_keyframe_id);
	Affine3d kf_pose;
	tf::poseMsgToEigen(msg->pose,kf_pose);
	Matrix3f pose_R = kf_pose.rotation().cast<float>();
	Vector3f pose_t = kf_pose.translation().cast<float>();

	kfPointCloud[cur_keyframe_id].SetModelMatrix(pose_R.data(), pose_t.data());
	kfShouldDraw[cur_keyframe_id] = true;
}
void trackerPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	Affine3d tracker_pose;
	tf::poseMsgToEigen(msg->pose,tracker_pose);
	Matrix3f tracker_pose_R = tracker_pose.rotation().cast<float>();
	Vector3f tracker_pose_t = tracker_pose.translation().cast<float>();
	float *tmp_R_ptr = tracker_pose_R.data();
	float *tmp_t_ptr = tracker_pose_t.data();;
	poseView.SetModelMatrix(tracker_pose_R.data(), tracker_pose_t.data());
	glutPostRedisplay();
}
void uavPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	Affine3d pose_uav_enu;
	tf::poseMsgToEigen(msg->pose,pose_uav_enu);
	Matrix3d att_g_enu = AngleAxisd (-M_PI/2, Vector3d::UnitX ()).toRotationMatrix();
	Affine3d pose_g_enu;
	pose_g_enu.linear() = att_g_enu;
	pose_g_enu.translation() = Vector3d::Zero();


	Affine3d pose_uav_g = pose_g_enu.inverse() *pose_uav_enu;
	pose_uav_g.translation() = Vector3d::Zero();
	cout<<"**************************************************"<<endl;
	cout<<pose_uav_enu.matrix()<<endl<<endl;
	cout<<pose_uav_g.matrix()<<endl<<endl;
	cout<<"**************************************************"<<endl;


	Matrix3f tracker_pose_R = pose_uav_g.rotation().cast<float>();
	Vector3f tracker_pose_t = pose_uav_g.translation().cast<float>();
	float *tmp_R_ptr = tracker_pose_R.data();
	float *tmp_t_ptr = tracker_pose_t.data();;
	poseView.SetModelMatrix(tracker_pose_R.data(), tracker_pose_t.data());
	glutPostRedisplay();
}




void GL_Display()
{
	glClearColor(0.5f,0.5f,0.5f,1.0f);
	glClear(GL_COLOR_BUFFER_BIT);
	glUniformMatrix4fv(render_projectionMatrix_position, 1, GL_FALSE, dCamera.GetProjectionMatrixDataPtr());
	glUniformMatrix4fv(render_viewMatrix_position, 1, GL_FALSE, dCamera.GetViewMatrixDataPtr());

	backgroundPointCloud.DrawPointCloud(render_modelMatrix_position);
	for(int i=0;i<MaxKeyFrameNum;i++)
	{
		if(kfShouldDraw[i]&&kfPointCloud[i].IsInited())
		{
			kfPointCloud[i].DrawPointCloud(render_modelMatrix_position);
		}
	}
	poseView.DrawPoseAsCam(render_modelMatrix_position);

	glutSwapBuffers();
}
void GL_MouseMotion(int x, int y)
{
	dCamera.Drag(x,y);
	glutPostRedisplay();
}
void GL_Mouse(int button, int state, int x, int y)
{
	if (state == GLUT_DOWN)
    {
		DOpenGLCamera::DragingType drag_type=DOpenGLCamera::DRAG_NONE;
		if(button == 0)
		{
			drag_type = DOpenGLCamera::DRAG_ROTATE;
		}
		else if (button == 2)
		{
			drag_type = DOpenGLCamera::DRAG_ZOOM;
		}
		else if(button == 1)
		{
			drag_type = DOpenGLCamera::DRAG_TRANSLATE;
		}
		dCamera.StartDrag(x,y,drag_type);
    }
    else if (state == GLUT_UP)
    {
		dCamera.StopDrag();
    }
    glutPostRedisplay();
}
bool InitGL()
{
	//OPENGL DISPLAY INIT
	int tmp_argc=0;
	glutInit(&tmp_argc, NULL);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
	const int WINDOW_WIDTH = 1040;
	glutInitWindowPosition(1920*2-WINDOW_WIDTH,1080-WINDOW_WIDTH);
	//glutInitWindowPosition(0,1080-WINDOW_WIDTH);
	glutInitWindowSize(WINDOW_WIDTH, WINDOW_WIDTH);
	glutInitContextVersion(4,5);
	glutInitContextProfile(GLUT_CORE_PROFILE);
	glutCreateWindow("Global View");
	glewExperimental = GL_TRUE;
	GLenum errorCode = glewInit();
	if(errorCode!=GLEW_OK)
	{
		cout<<"Error initing glew: "<<glewGetErrorString(errorCode)<<"  Errorcode:["<<errorCode<<"]"<<endl<<"Exiting..."<<endl;

		return false;
	}
	fprintf(stdout, "Using GLEW %s\n", glewGetString(GLEW_VERSION));


	glutDisplayFunc(GL_Display);
	glutKeyboardFunc(NULL);
	glutMouseFunc(GL_Mouse);
	glutMotionFunc(GL_MouseMotion);
	glutReshapeFunc(NULL);
	glutIdleFunc(NULL);
	glutCloseFunc(NULL);


	string package_path = ros::package::getPath("kinfu_slam");
	string shader_path = package_path+"/src/Shaders/";
	string vertex_shader_path = shader_path + "VertexShader_ColoredByColorAndNorm.txt";
	string fragment_shader_path = shader_path + "FragmentShader.txt";

	ROS_INFO("Loading vertex shader : %s",vertex_shader_path.data());
	ROS_INFO("Loading fragment shader : %s",fragment_shader_path.data());
	ShaderInfo shaders[] = {
		{GL_VERTEX_SHADER, vertex_shader_path.data()},
		{GL_FRAGMENT_SHADER, fragment_shader_path.data()},
		{GL_NONE, NULL}
	};
	ShadingProgram = LoadShaders(shaders);
	if(ShadingProgram==0)
	{
		ROS_ERROR("Loading shader failed");
		return false;
	}
	ROS_INFO("Loading shader successful");
	glUseProgram(ShadingProgram);


	render_projectionMatrix_position  = glGetUniformLocation(ShadingProgram, "projectionMatrix");
	render_viewMatrix_position = glGetUniformLocation(ShadingProgram, "viewMatrix");
	render_modelMatrix_position = glGetUniformLocation(ShadingProgram, "modelMatrix");
	glClearColor(0,0,0,1.0);


	return true;
}

void initPointcloud()
{
	backgroundPointCloud.Init();
	for(int i=0;i<MaxKeyFrameNum;i++)
	{
		kfPointCloud[i].Init();
	}

	int vIdx=0;
	//x
	/*
	for(int y=-StaticDisplayCubeLength_Y;y<StaticDisplayCubeLength_Y;y++)
	{
		for(int z=-StaticDisplayCubeLength_Z;z<StaticDisplayCubeLength_Z;z++)
		{
			staticDisplayCubeVertexList[vIdx*3+0] = 0 * StaticDisplayCubeRes;
			staticDisplayCubeVertexList[vIdx*3+1] = y * StaticDisplayCubeRes;
			staticDisplayCubeVertexList[vIdx*3+2] = z * StaticDisplayCubeRes;
			staticDisplayCubeNormList[vIdx*3+0]=-1;
			staticDisplayCubeNormList[vIdx*3+1]=0;
			staticDisplayCubeNormList[vIdx*3+2]=0;
			staticDisplayCubeColorList[vIdx*3+0] = 1;
			staticDisplayCubeColorList[vIdx*3+1] = 0;
			staticDisplayCubeColorList[vIdx*3+2] = 0;

			vIdx++;
		}
	}
	*/
	//y
	for(int x=-StaticDisplayCubeLength_X;x<StaticDisplayCubeLength_X;x++)
	{
		for(int z=-StaticDisplayCubeLength_Z;z<StaticDisplayCubeLength_Z;z++)
		{
			staticDisplayCubeVertexList[vIdx*3+0] = x * StaticDisplayCubeRes;
			staticDisplayCubeVertexList[vIdx*3+1] = 2;
			staticDisplayCubeVertexList[vIdx*3+2] = z * StaticDisplayCubeRes;
			staticDisplayCubeNormList[vIdx*3+0]=std::numeric_limits<float>::quiet_NaN();
			//staticDisplayCubeNormList[vIdx*3+0]=0;
			staticDisplayCubeNormList[vIdx*3+1]=-1;
			staticDisplayCubeNormList[vIdx*3+2]=0;

			staticDisplayCubeColorList[vIdx*3+0] = 0;
			//staticDisplayCubeColorList[vIdx*3+0]=std::numeric_limits<float>::quiet_NaN();
			staticDisplayCubeColorList[vIdx*3+1] = 1;
			staticDisplayCubeColorList[vIdx*3+2] = 0;
			vIdx++;
		}
	}
	//z
	/*
	for(int x=-StaticDisplayCubeLength_X;x<StaticDisplayCubeLength_X;x++)
	{
		for(int y=-StaticDisplayCubeLength_Y;y<StaticDisplayCubeLength_Y;y++)
		{
			staticDisplayCubeVertexList[vIdx*3+0] = x * StaticDisplayCubeRes;
			staticDisplayCubeVertexList[vIdx*3+1] = y * StaticDisplayCubeRes;
			staticDisplayCubeVertexList[vIdx*3+2] = 0 * StaticDisplayCubeRes;
			staticDisplayCubeNormList[vIdx*3+0]=0;
			staticDisplayCubeNormList[vIdx*3+1]=0;
			staticDisplayCubeNormList[vIdx*3+2]=-1;

			staticDisplayCubeColorList[vIdx*3+0] = 0;
			staticDisplayCubeColorList[vIdx*3+1] = 0;
			staticDisplayCubeColorList[vIdx*3+2] = 1;
			vIdx++;
		}
	}
	*/
	backgroundPointCloud.LoadPointCloud(staticDisplayCubeVertexList,3,vIdx,GL_DYNAMIC_DRAW);
	backgroundPointCloud.LoadPointCloudNorm(staticDisplayCubeNormList,3,vIdx,GL_DYNAMIC_DRAW);
	backgroundPointCloud.LoadPointCloudColor(staticDisplayCubeColorList,3,vIdx,GL_DYNAMIC_DRAW);
	memset(kfShouldDraw,0,sizeof(bool) * MaxKeyFrameNum);
	memset(pointcloudLoaded,0,sizeof(bool) * MaxKeyFrameNum);

}
